#ifndef SAY_SOMETHING_NODE_HPP_
#define SAY_SOMETHING_NODE_HPP_

#include <behaviortree_cpp/bt_factory.h>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"      // 包含生命周期节点库
#include "lifecycle_msgs/msg/transition.hpp"        // 包含生命周期消息中的转换消息
#include "rclcpp_lifecycle/lifecycle_publisher.hpp" // 包含生命周期发布者库
#include "test_interfaces/srv/test.hpp"

#include "MyLifecycleNode.hpp"

using namespace std::chrono_literals;

/**
 * @brief  StatefullActionNode 可以返回RUNNING状态
 */
class SaySomethingNode : public BT::StatefulActionNode
{
private:
    std::shared_ptr<MyLifecycleNode> node_;

public:
    SaySomethingNode(const std::string &name,
                     const BT::NodeConfig &config,
                     std::shared_ptr<MyLifecycleNode> node)
        : BT::StatefulActionNode(name, config), node_(node)
    {
    }

    ~SaySomethingNode()
    {
    }

    static BT::PortsList providedPorts()
    {
        return {BT::InputPort<std::string>("message")};
    }

    BT::NodeStatus onStart() override
    {
        node_->send_request();
        return BT::NodeStatus::RUNNING;
    }

    bool isFinished()
    {
        return node_->isFinished();
    }

    BT::NodeStatus onRunning() override
    {
        if (!isFinished())
        {
            return BT::NodeStatus::RUNNING;
        }
        else
        {
            auto msg = getInput<std::string>("message");
            if (!msg)
            {
                throw BT::RuntimeError("missing parameter [message]");
            }

            std::cout << "say: " << msg.value() << "add result: " << node_->get_result() << std::endl;
            return BT::NodeStatus::SUCCESS;
        }
    }

    void onHalted() override
    {
    }
};

#endif